package ray.math;

/**
 * This class represents a generic homogenous transformation. It has convenience methods in it for multiplication with other
 * transformations, for transforming vectors, for creating rotation, scaling, and translational transformations, as well as camer
 * to frame transformations and vice versa.
 * 
 * @author ags
 */
public class Matrix4 {

    // the 16 entries
    public double[][] m = new double[4][4];

    /**
     * The default constructor initializes the matrix to I.
     */
    public Matrix4() {

        setIdentity();
    }

    public Matrix4(Matrix4 m) {
        set(m);
    }

    /**
     * Copies the values in the parameter into this matrix.
     * 
     * @param newM
     *            The values to copy.
     */
    public void set(double[][] newM) {

        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                m[i][j] = newM[i][j];
    }

    /**
     * Copies the values from one matrix into this matrix.
     * 
     * @param newMat
     *            The matrix to copy.
     */
    public void set(Matrix4 newMat) {

        set(newMat.m);
    }

    /**
     * Sets this matrix to the identity matrix.
     */
    public void setIdentity() {

        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                m[i][j] = (i == j) ? 1.0f : 0.0f;
    }
    public void setCtoF(Vector3 u, Vector3 v, Vector3 w, Vector3 p) {

        setIdentity();
        m[0][0] = u.x;
        m[0][1] = u.y;
        m[0][2] = u.z;
        m[1][0] = v.x;
        m[1][1] = v.y;
        m[1][2] = v.z;
        m[2][0] = w.x;
        m[2][1] = w.y;
        m[2][2] = w.z;
        Vector3 temp = new Vector3(p.x, p.y, p.z);
        rightMultiply(temp);
        m[0][3] = -temp.x;
        m[1][3] = -temp.y;
        m[2][3] = -temp.z;
    }

    public void setFtoC(Vector3 u, Vector3 v, Vector3 w, Vector3 p) {

        setIdentity();
        m[0][0] = u.x;
        m[0][1] = v.x;
        m[0][2] = w.x;
        m[0][3] = p.x;
        m[1][0] = u.y;
        m[1][1] = v.y;
        m[1][2] = w.y;
        m[1][3] = p.y;
        m[2][0] = u.z;
        m[2][1] = v.z;
        m[2][2] = w.z;
        m[2][3] = p.z;
    }

    /**
     * This sets the current matrix to a rotation about the indicated axis.
     * 
     * @param angle
     *            The magnitude (in degrees) of the rotation.
     * @param axis
     *            The axis about which the rotation will occur.
     */
    public void setRotate(double angleDegrees, Vector3 axis) {
//        double angleRadians = angleDegrees * Math.PI / 180;

        // nth change
        double angleRadians = Math.toRadians(angleDegrees);
        Vector3 u = new Vector3(axis);
        u.normalize();
        double[] ua = new double[] { u.x, u.y, u.z };
        setIdentity();
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                m[i][j] = ua[i] * ua[j];
            }
        }
        
        double cosTheta = (double) Math.cos(angleRadians);
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                m[i][j] += cosTheta * ((i == j ? 1.0f : 0.0f) - ua[i] * ua[j]);
            }
        }
        
        double sinTheta = (double) Math.sin(angleRadians);
        m[1][2] -= sinTheta * u.x;
        m[2][1] += sinTheta * u.x;
        m[2][0] -= sinTheta * u.y;
        m[0][2] += sinTheta * u.y;
        m[0][1] -= sinTheta * u.z;
        m[1][0] += sinTheta * u.z;
    }

    /**
     * This sets the matrix to a translation transformation.
     * 
     * @param translationFactor
     *            The vector describing the translation.
     */
    public void setTranslate(Vector3 translationFactor) {
        setIdentity();
        m[0][3] = translationFactor.x;
        m[1][3] = translationFactor.y;
        m[2][3] = translationFactor.z;
    }

    /**
     * This sets the matrix to a scaling transformation.
     * 
     * @param scaleFactor
     *            The vector that describes the scaling.
     */
    public void setScale(Vector3 scaleFactor) {
        setIdentity();
        m[0][0] = scaleFactor.x;
        m[1][1] = scaleFactor.y;
        m[2][2] = scaleFactor.z;
    }

    /** Temporary storage for doing a matrix multiply */
    private double[][] tempM = new double[4][4];

    /**
     * Concatenates t onto this matrix, and places the result in the second parameter. result = t*this. There are no restrictions
     * of the form t != result, or result != this, etc.
     * 
     * @param t
     *            The left operand.
     * @param result
     *            The destination for the result.
     */
    public void leftMultiply(Matrix4 t, Matrix4 result) {
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {

                tempM[i][j] = 0;
                for (int k = 0; k < 4; k++) {
                    tempM[i][j] += t.m[i][k] * m[k][j];
                }

            }
        }

        result.set(tempM);
    }

    /**
     * Sets this matrix to be the left product of t and itself. This = t*this.
     * 
     * @param t
     *            The left hand operand.
     */
    public void leftCompose(Matrix4 t) {
        leftMultiply(t, this);
    }

    /**
     * Concatenates this matrix onto t, and places the result in the second parameter. result = this(t. There are no restrictions
     * of the form t != result, or result != this, etc.
     * 
     * @param t
     *            The right hand operand
     * @param result
     *            The destination for the result.
     */
    public void rightMultiply(Matrix4 t, Matrix4 result) {
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                tempM[i][j] = 0;
                for (int k = 0; k < 4; k++) {
                    tempM[i][j] += m[i][k] * t.m[k][j];
                }
            }
        }

        result.set(tempM);
    }

    /**
     * Sets this matrix to be the right product of t and itself. This = this*t.
     * 
     * @param t
     *            The right hand operand.
     */
    public void rightCompose(Matrix4 t) {
        rightMultiply(t, this);
    }

    /**
     * Transforms the vector by the current matrix and stores the result in the given vector.
     * 
     * @param vInOut
     *            The vector to transform, and the destination vector.
     */
    public void rightMultiply(Vector3 vInOut) {
        rightMultiply(vInOut, vInOut);
    }

    /**
     * Transforms the v vector by this matrix, and places the result in vOut. v and vOut may reference the same object.
     * Transforming a vector means transforming by the upper 3x3.
     * 
     * @param v
     *            The vector to transform.
     * @param vOut
     *            The destination vector.
     */
    public void rightMultiply(Vector3 v, Vector3 vOut) {
        double x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z;
        double y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z;
        double z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z;
        vOut.set(x, y, z);
    }

    /**
     * Transforms the point by the current matrix and stores the result in the given point.
     * 
     * @param vInOut
     *            The point to transform, and the destination point.
     */
    public void rightMultiply(Point3 vInOut) {
        rightMultiply(vInOut, vInOut);
    }

    /**
     * Transforms the v point by this matrix, and places the result in vOut. v and vOut may reference the same object.
     * 
     * @param v
     *            The point to transform.
     * @param vOut
     *            The destination point.
     */
    public void rightMultiply(Point3 v, Point3 vOut) {
        double x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3];
        double y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3];
        double z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3];
        double w = m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3];
        vOut.set(x / w, y / w, z / w);
    }

    /**
     * Sets the value of this matrix to the transpose of the passed matrix m1. Safe when m1 == this.
     * 
     * @param m1
     *            the matrix to be transposed
     */
    public void transpose(Matrix4 m1) {
        for (int r = 0; r < 3; r++)
            for (int c = r; c < 4; c++) {
                double temp = m1.m[r][c];
                m[r][c] = m1.m[c][r];
                m[c][r] = temp;
            }
    }

    /**
     * Transposes this matrix in place.
     */
    public void transpose() {
        transpose(this);
    }

    //
    // The following is re-used from the source code of the vecmath Vector4d class.
    //

    /**
     * Sets the value of this matrix to the matrix inverse of the passed matrix m1.
     * 
     * @param m1
     *            the matrix to be inverted
     */
    public void invert(Matrix4 m1) {
        invertGeneral(m1);
    }

    /**
     * Inverts this matrix in place.
     */
    public void invert() {
        invertGeneral(this);
    }

    /**
     * General invert routine. Inverts m1 and places the result in "this". Note that this routine handles both the "this" version
     * and the non-"this" version.
     * 
     * Also note that since this routine is slow anyway, we won't worry about allocating a little bit of garbage.
     */
    final void invertGeneral(Matrix4 m1) {
        double result[] = new double[16];
        int row_perm[] = new int[4];
        int i, r, c;

        // Use LU decomposition and backsubstitution code specifically
        // for floating-point 4x4 matrices.
        double[] tmp = new double[16]; // scratch matrix

        // Copy source matrix to t1tmp
        for (i = 0, r = 0; r < 4; r++) {
            for (c = 0; c < 4; c++, i++) {
                tmp[i] = m1.m[r][c];
            }
        }

        // Calculate LU decomposition: Is the matrix singular?
        boolean nonsingular = luDecomposition(tmp, row_perm);
        assert nonsingular;

        // Perform back substitution on the identity matrix
        for (i = 0; i < 16; i++) {
            result[i] = 0.0;
        }
        result[0] = 1.0;
        result[5] = 1.0;
        result[10] = 1.0;
        result[15] = 1.0;
        luBacksubstitution(tmp, row_perm, result);

        // Copy result into the output array
        for (i = 0, r = 0; r < 4; r++) {
            for (c = 0; c < 4; c++, i++) {
                this.m[r][c] = result[i];
            }
        }
    }

    /**
     * Given a 4x4 array "matrix0", this function replaces it with the LU decomposition of a row-wise permutation of itself. The
     * input parameters are "matrix0" and "dimen". The array "matrix0" is also an output parameter. The vector "row_perm[4]" is an
     * output parameter that contains the row permutations resulting from partial pivoting. The output parameter "even_row_xchg"
     * is 1 when the number of row exchanges is even, or -1 otherwise. Assumes data type is always double.
     * 
     * This function is similar to luDecomposition, except that it is tuned specifically for 4x4 matrices.
     * 
     * @return true if the matrix is nonsingular, or false otherwise.
     */
    //
    // Reference: Press, Flannery, Teukolsky, Vetterling,
    // _Numerical_Recipes_in_C_, Cambridge University Press,
    // 1988, pp 40-45.
    //
    static boolean luDecomposition(double[] matrix0, int[] row_perm) {
        double row_scale[] = new double[4];

        // Determine implicit scaling information by looping over rows
        {
            int i, j;
            int ptr, rs;
            double big, temp;

            ptr = 0;
            rs = 0;

            // For each row ...
            i = 4;
            while (i-- != 0) {
                big = 0.0;

                // For each column, find the largest element in the row
                j = 4;
                while (j-- != 0) {
                    temp = matrix0[ptr++];
                    temp = Math.abs(temp);
                    if (temp > big) {
                        big = temp;
                    }
                }

                // Is the matrix singular?
                if (big == 0.0) {
                    return false;
                }
                row_scale[rs++] = 1.0 / big;
            }
        }

        {
            int j;
            int mtx;

            mtx = 0;

            // For all columns, execute Crout's method
            for (j = 0; j < 4; j++) {
                int i, imax, k;
                int target, p1, p2;
                double sum, big, temp;

                // Determine elements of upper diagonal matrix U
                for (i = 0; i < j; i++) {
                    target = mtx + (4 * i) + j;
                    sum = matrix0[target];
                    k = i;
                    p1 = mtx + (4 * i);
                    p2 = mtx + j;
                    while (k-- != 0) {
                        sum -= matrix0[p1] * matrix0[p2];
                        p1++;
                        p2 += 4;
                    }
                    matrix0[target] = sum;
                }

                // Search for largest pivot element and calculate
                // intermediate elements of lower diagonal matrix L.
                big = 0.0;
                imax = -1;
                for (i = j; i < 4; i++) {
                    target = mtx + (4 * i) + j;
                    sum = matrix0[target];
                    k = j;
                    p1 = mtx + (4 * i);
                    p2 = mtx + j;
                    while (k-- != 0) {
                        sum -= matrix0[p1] * matrix0[p2];
                        p1++;
                        p2 += 4;
                    }
                    matrix0[target] = sum;

                    // Is this the best pivot so far?
                    if ((temp = row_scale[i] * Math.abs(sum)) >= big) {
                        big = temp;
                        imax = i;
                    }
                }

                assert (imax >= 0);

                // Is a row exchange necessary?
                if (j != imax) {
                    // Yes: exchange rows
                    k = 4;
                    p1 = mtx + (4 * imax);
                    p2 = mtx + (4 * j);
                    while (k-- != 0) {
                        temp = matrix0[p1];
                        matrix0[p1++] = matrix0[p2];
                        matrix0[p2++] = temp;
                    }

                    // Record change in scale factor
                    row_scale[imax] = row_scale[j];
                }

                // Record row permutation
                row_perm[j] = imax;

                // Is the matrix singular
                if (matrix0[(mtx + (4 * j) + j)] == 0.0) {
                    return false;
                }

                // Divide elements of lower diagonal matrix L by pivot
                if (j != (4 - 1)) {
                    temp = 1.0 / (matrix0[(mtx + (4 * j) + j)]);
                    target = mtx + (4 * (j + 1)) + j;
                    i = 3 - j;
                    while (i-- != 0) {
                        matrix0[target] *= temp;
                        target += 4;
                    }
                }
            }
        }

        return true;
    }

    /**
     * Solves a set of linear equations. The input parameters "matrix1", and "row_perm" come from luDecompostionD4x4 and do not
     * change here. The parameter "matrix2" is a set of column vectors assembled into a 4x4 matrix of floating-point values. The
     * procedure takes each column of "matrix2" in turn and treats it as the right-hand side of the matrix equation Ax = LUx = b.
     * The solution vector replaces the original column of the matrix.
     * 
     * If "matrix2" is the identity matrix, the procedure replaces its contents with the inverse of the matrix from which
     * "matrix1" was originally derived.
     */
    //
    // Reference: Press, Flannery, Teukolsky, Vetterling,
    // _Numerical_Recipes_in_C_, Cambridge University Press,
    // 1988, pp 44-45.
    //
    static void luBacksubstitution(double[] matrix1, int[] row_perm, double[] matrix2) {

        int i, ii, ip, j, k;
        int rp;
        int cv, rv;

        // rp = row_perm;
        rp = 0;

        // For each column vector of matrix2 ...
        for (k = 0; k < 4; k++) {
            // cv = &(matrix2[0][k]);
            cv = k;
            ii = -1;

            // Forward substitution
            for (i = 0; i < 4; i++) {
                double sum;

                ip = row_perm[rp + i];
                sum = matrix2[cv + 4 * ip];
                matrix2[cv + 4 * ip] = matrix2[cv + 4 * i];
                if (ii >= 0) {
                    // rv = &(matrix1[i][0]);
                    rv = i * 4;
                    for (j = ii; j <= i - 1; j++) {
                        sum -= matrix1[rv + j] * matrix2[cv + 4 * j];
                    }
                } else if (sum != 0.0) {
                    ii = i;
                }
                matrix2[cv + 4 * i] = sum;
            }

            // Backsubstitution
            // rv = &(matrix1[3][0]);
            rv = 3 * 4;
            matrix2[cv + 4 * 3] /= matrix1[rv + 3];

            rv -= 4;
            matrix2[cv + 4 * 2] = (matrix2[cv + 4 * 2] - matrix1[rv + 3] * matrix2[cv + 4 * 3]) / matrix1[rv + 2];

            rv -= 4;
            matrix2[cv + 4 * 1] = (matrix2[cv + 4 * 1] - matrix1[rv + 2] * matrix2[cv + 4 * 2] - matrix1[rv + 3]
                    * matrix2[cv + 4 * 3])
                    / matrix1[rv + 1];

            rv -= 4;
            matrix2[cv + 4 * 0] = (matrix2[cv + 4 * 0] - matrix1[rv + 1] * matrix2[cv + 4 * 1] - matrix1[rv + 2]
                    * matrix2[cv + 4 * 2] - matrix1[rv + 3] * matrix2[cv + 4 * 3])
                    / matrix1[rv + 0];
        }
    }

    /**
     * @see java.lang.Object#toString()
     */
    public String toString() {

        return "Matrix4f: [" + m[0][0] + "  " + m[0][1] + "  " + m[0][2] + "  " + m[0][3] + "\n" + "           " + m[1][0] + "  "
                + m[1][1] + "  " + m[1][2] + "  " + m[1][3] + "\n" + "           " + m[2][0] + "  " + m[2][1] + "  " + m[2][2]
                + "  " + m[2][3] + "\n" + "           " + m[3][0] + "  " + m[3][1] + "  " + m[3][2] + "  " + m[3][3] + "]\n";
    }

    public static Matrix4 getIdentity() {
        Matrix4 result = new Matrix4();
        result.setIdentity();
        return result;
    }

}
